#include "qs_flash.h"
#include "qs_config.h"
#include "qs_log.h"
#include "main.h"

static __no_init qs_mem_store_t m_motor_info;

/**
  * @brief  FLASH erase function
  * @param  None
  * @retval None
  */
static void APP_FlashErase(void)
{
  uint32_t PAGEError = 0;
  FLASH_EraseInitTypeDef EraseInitStruct = {0};

  EraseInitStruct.TypeErase   = FLASH_TYPEERASE_PAGEERASE;        /* Erase type (FLASH_TYPEERASE_PAGES for page erase, FLASH_TYPEERASE_SECTORS for sector erase) */
  EraseInitStruct.PageAddress = FLASH_USER_START_ADDR;            /* Erase start address */
  EraseInitStruct.NbPages  = 1;      /* Number of pages to be erased */
  if (HAL_FLASHEx_Erase(&EraseInitStruct, &PAGEError) != HAL_OK)  /* Execute page erase, PAGEError returns the page with erase error, 0xFFFFFFFF indicates successful erase */
  {
    APP_ErrorHandler();
  }
}

/**
  * @brief  FLASH programming function
  * @param  None
  * @retval None
  */
static void APP_FlashProgram(void)
{
  uint32_t flash_program_start = FLASH_USER_START_ADDR ;                /* Start address for flash programming */
  uint32_t flash_program_end = (FLASH_USER_START_ADDR + sizeof(m_motor_info));  /* End address for flash programming */
  uint32_t *src = (uint32_t *)&m_motor_info;                                     /* Array pointer */

  while (flash_program_start < flash_program_end)
  {
    if (HAL_FLASH_Program(FLASH_TYPEPROGRAM_PAGE, flash_program_start, src) == HAL_OK)/* Execute programming */
    {
      flash_program_start += FLASH_PAGE_SIZE; /* Point flash start address to the next word */
      src += FLASH_PAGE_SIZE / 4;             /* Update data pointer */
    }
  }
}

/**
  * @brief  FLASH blank check function
  * @param  None
  * @retval None
  */
static void APP_FlashBlank(void)
{
  uint32_t addr = 0;

  while (addr < sizeof(m_motor_info))
  {
    if (0xFFFFFFFF != HW32_REG(FLASH_USER_START_ADDR + addr))
    {
      APP_ErrorHandler();
    }
    addr += 4;
  }
}

/**
  * @brief  FLASH verification function
  * @param  None
  * @retval None
  */
static void APP_FlashVerify(void)
{
  uint32_t addr = 0;
	uint32_t *src = (uint32_t *)&m_motor_info; 
  while (addr < sizeof(m_motor_info))
  {
    if (*src != HW32_REG(FLASH_USER_START_ADDR + addr))
    {
      QS_LOG_INFO("Pulse storage error!\r\n");
			break;
    }
    addr += 4;
		src++;
  }
}

void qs_pulse_storage(qs_mem_store_t *motor_info)
{

    memcpy(&m_motor_info, motor_info,sizeof(qs_mem_store_t));
    /* Unlock the Flash to enable the flash control register access */
    HAL_FLASH_Unlock();

    /* Erase Flash */
    APP_FlashErase();

    /* Check Flash if blank */
    APP_FlashBlank();

    /* Write user data to Flash */
    APP_FlashProgram();

    /* Lock the Flash to disable the flash control register access */
    HAL_FLASH_Lock();
		
		APP_FlashVerify();

}

qs_storage_status_t qs_pulse_read(qs_mem_store_t *motor_info)
{
	  uint32_t addr = 0;
	  uint8_t i = 0;
//	  uint32_t src = (uint32_t *)motor_info; 
    while (addr < sizeof(qs_mem_store_t))
    {
			uint32_t flash_data = HW32_REG(FLASH_USER_START_ADDR + addr);
			
			if(flash_data != 0xFFFFFFFF) {

				if(addr == (sizeof(qs_mem_store_t) - 4)) {
					motor_info->motor_dir =    (uint8_t) (flash_data  >> 24);
					motor_info->motor_motion = (uint8_t) (flash_data  >> 16);
				}
				else
					motor_info->mem_pulse[i] = *(float*)&flash_data;
				
        addr += 4;
				i ++;
			}else {
			  QS_LOG_INFO("Pulses not storaged!\r\n");
				return STORAGE_INACTIVE;
			}

    }
		return STORAGE_ACTIVE;
}

